#ifndef _SIUT_SIMD_QUATERNION_HPP_
#define _SIUT_SIMD_QUATERNION_HPP_
/**
 * \file Quaternion.hpp
 * Header that selects the standard Quaternion.hpp
 */

#include "Quatf.hpp"
#include "Mat4f.hpp"


namespace siut {
  namespace simd {
    
    /** Create a quaternion with specified axis and angle.
     * \param v Vector of rotation
     * \param angle Angle of rotation.
     *
     * Returns a quaternion built the following way:
     * Quatf(cos(0.5*angle), sin(0.5 * v));
     */
    inline Quatf quatFromAxisAngleRotation(const Vec3f &v, float angle)
    {
      float sinA = static_cast<float>(std::sin(0.5*angle));
      float cosA = static_cast<float>(std::cos(0.5*angle));
#ifndef NVCC	
      return Quatf(cosA, sinA*v[0], sinA*v[1], sinA*v[2]);
#else
      Quatf q;
      setScalar(q, cosA);
      setVector(q, sinA*v[0], sinA*v[1], sinA*v[2]);
      return q;
#endif
    }


    /** Create a quaternion describing great circle from a to b. */

    inline Quatf quatFromGreatCircle(const Vec3f &a, const Vec3f &b)
    {

      float angle = acos(a.dot(b));
      float cosA = static_cast<float>(std::cos(0.5*angle));
      float sinA = static_cast<float>(std::sin(0.5*angle));

      Vec3f axis = a.cross(b);
      axis.normalize();       
      axis *= sinA;

#ifndef NVCC
      return Quatf(cosA, axis);
#else
      Quatf q;
      setScalar(q, cosA);
      setVector(q, axis);
      return q;
#endif       
    }


    /** Generate a quaternion from a Mat4f
     * Based on http://www.gamasutra.com/features/19980703/quaternions_01.htm
     * and libdyk
     * If DEBUG_QUAT is defined, will throw a runtime_error if there is a problem with creating the quaternion.
     */
    inline Quatf quatFromMatrix(const Mat4f &m)
    {
      Quatf q;
      float diag = get(m, 0) + get(m, 5) + get(m,10);
      float s;
      size_t i, j, k;
      if(diag > 0.0) {
	s = static_cast<float>(std::sqrt(diag +1.0));
	q.set(0, s / 2.0f);
	q.set(1, (get(m,6) - get(m, 9)) / (2.f * s));
	q.set(2, (get(m ,8) - get(m, 2)) / (2.f * s));
	q.set(3, (get(m, 1) - get(m, 4)) / (2.f * s));
      } else {
	i = 0;
	if(get(m, 5) > get(m, 0))
	  i = 1;
	if(get(m,10) > get(m,i, i))
	  i = 2;
	j = (i+1)%3;
	k = (i+2)%3;
		
	s = static_cast<float>(std::sqrt(get(m,i, i) - get(m, j, j) - get(m, k, k) + 1.0));
#ifdef DEBUG_QUAT
	if(std::fabs(s) < std::numeric_limits<float>::epsilon())
	  {
	    std::stringstream s;
	    s << __func__ << " s has too small value in " << __FILE__ << " at " << __LINE__<<std::endl;
	    throw std::runtime_error(s.str());
	  }
#endif
	q.set(i+1, s / 2.f);
	q.set(0, (get(m, j, k) - get(m, k, j)) / (2.f * s));
	q.set(j+1, (get(m, i, j) + get(m, j, i)) / (2.f * s));
	q.set(k+1, (get(m, i, k) + get(m, k, i)) / (2.f * s));

      }
      return q;

    }
      

    /** Builds a Mat4f from the quaternion.
     * Based on http://www.gamasutra.com/features/19980703/quaternions_01.htm
     */
    inline Mat4f quatToMat4f(const Quatf &a)
    {
      Mat4f m;

      float xpx, ypy, zpz;
      xpx = a.x() + a.x(); 
      ypy = a.y() + a.y();
      zpz = a.z() + a.z();

      float zz, yz, yy, xz, xy, xx, wz, wy, wx;
      zz = a.z() * zpz;
      yz = a.y() * zpz; yy = a.y() * ypy;
      xz = a.x() * zpz; xy = a.x() * ypy; xx = a.x() * xpx;
      wz = a.w() * zpz; wy = a.w() * ypy; wx = a.w() * xpx;

      set(m,0, 1.0f - (yy + zz));
      set(m,1, xy + wz);
      set(m,2, xz - wy);
      set(m,3, 0.0f);
      set(m, 4, xy - wz);
      set(m, 5, 1.0f - (xx + zz));
      set(m, 6, yz + wx);
      set(m, 7, 0.f);
      set(m, 8, xz + wy); 
      set(m, 9, yz - wx);
      set(m, 10, 1.0f - (xx + yy));
      set(m, 11, 0.0f);
      set(m, 12, 0.0f);
      set(m, 13, 0.0f);
      set(m, 14, 0.0f);
      set(m, 15, 1.0f);

	
      return m;
    }


#ifdef NVCC
    /** Multiply a quaternion with a float3 vector.
     * Assumes vector is a quaternion without w component.
     */
    inline CUDAHD Quatf operator * (const Quatf &a, const float3 &b)
#else
    /** Multiply a quaternion with a Vec3f.
     * Assumes vector is a quaternion without w component.
     */
      inline Quatf operator * (const Quatf &a, const Vec3f &b)
#endif
    {
      Quatf q;	      
      q.q_[0] = -(a.q_[1]*b[0] + a.q_[2]*b[1] + a.q_[3]*b[2]);
      q.q_[1] = a.q_[2]*b[2] - a.q_[3]*b[1] + a.q_[0]*b[0];
      q.q_[2] = a.q_[3]*b[0] - a.q_[1]*b[2] + a.q_[0]*b[1];
      q.q_[3] = a.q_[1]*b[1] - a.q_[2]*b[0] + a.q_[0]*b[2];
	      
      return q;
    }

#ifdef NVCC
    /** Multiply a quaternion with a float3 vector.
     * Assumes vector is a quaternion without w component.
     */
    inline CUDAHD Quatf operator * (const float3 &a, const Quatf &b)
#else
    /** Multiply a quaternion with a Vec3f.
     * Assumes vector is a quaternion without w component.
     */
      inline Quatf operator * (const Vec3f &a, const Quatf &b)
#endif
    {
      Quatf q;	      
	
      q.q_[0] = -(a[0]*b.q_[1] + a[1]*b.q_[2] + a[2]*b.q_[3]);
      q.q_[1] = a[1]*b.q_[3] - a[2]*b.q_[2] + a[0]*b.q_[0];
      q.q_[2] = a[2]*b.q_[1] - a[0]*b.q_[3] + a[1]*b.q_[0];
      q.q_[3] = a[0]*b.q_[2] - a[1]*b.q_[1] + a[2]*b.q_[0];

      return q;
    }


#ifdef NVCC
    /** Rotates quaternion with the vector. */
    inline CUDAHD float3 rotateVec (const Quatf &a, const float3 v)
#else
    /** Rotates quaternion with the vector. */
      inline CUDAHD Vec3f rotateVec(const Quatf &a, const Vec3f v)
#endif
    {
      Quatf (q);
      q *= (~a)* v*a;
      return q.getVector();
    }


#ifdef NVCC
    /** Rotates quaternion with the vector. */
    inline CUDAHD float3 rotateVec (const float3 v, const Quatf &a)
#else
    /** Rotates quaternion with the vector. */
      inline CUDAHD Vec3f rotateVec(const Vec3f v, const Quatf &a)
#endif
    {
      Quatf q;
      q = a* v * (~a);
      return q.getVector();
    }


  } //namespace simd
} //namespace siut
#endif
